SuperStrict
Framework BRL.GlMax2D
Import brl.Random

AppTitle = "Marching Squares"
Global resX:Int=800, resY:Int=600
SetGraphicsDriver GLMax2DDriver()
Graphics resX,resY,0,0

SeedRnd(0)'MilliSecs() 

Local numBalls:Int = 10
Local balls:tball[numBalls]
For Local i:Int=0 Until numBalls
	balls[i] = TBall.Create( TPoint.Create( Rand(150,resX-150), Rand(150,resY-150) ), 1 )'Rand(1,4)
Next

Local threshold:Double = 0.1 '1e-5
Local mbs:tmetaballsystem = TMetaballsystem.Create(balls, 3, threshold) 'balls, gooeyness, and potential field threshold

Local iterations:Int = 0
Local fps:Int = 0
Local lastSec:Long = MilliSecs() + 1000
Local frames:Int = 0
Local closestBall:TBall = Null

While(Not KeyHit(KEY_ESCAPE) And Not AppTerminate()	)
	Cls

	If(MouseDown(1) And closestBall <> Null)
		closestBall.pos.x = MouseX()
		closestBall.pos.y = MouseY() 
	Else
		'Find ball closest to mouse
		Local minDist# = 1e32
		For Local b:TBall = EachIn balls
			Local distSq# = (MouseX() - b.pos.x)^2 + (MouseY() - b.pos.y)^2
			If(distSq < minDist)
				closestBall = b
				minDist = distSq
			EndIf
		Next
	EndIf
	
	mbs.marchingSquares()
	
	For Local b:TBall = EachIn balls
		DrawRect b.pos.x-2, b.pos.y-2, 4,4
	Next
	
	SetColor 255,255,255
	If(closestBall <> Null) Then DrawLine(MouseX(), MouseY(), closestBall.pos.x, closestBall.pos.y )
	DrawText("iterations: "+iterations+"    fps: "+fps, 0,0)
	
	If(lastSec < MilliSecs() )
		lastSec = MilliSecs() + 250
		fps = frames*4
		frames = 0
	EndIf
	
	iterations:+1
	frames :+ 1
	Flip
Wend
End

Type TPoint
	Field x:Double, y:Double
	
	Function Create:TPoint(x:Double, y:Double)
		Local tmp:TPoint = New TPoint
		tmp.x = x
		tmp.y = y
		Return tmp
	End Function
	
	Method clone:TPoint()
		Local tmp:TPoint = New TPoint
		tmp.x = x
		tmp.y = y
		Return tmp
	End Method
	
	Method set(x:Double, y:Double)
		Self.x = x
		Self.y = y
	End Method
	
	Method add(other:TPoint)
		Self.x :+ other.x; Self.y :+ other.y
	End Method
End Type

Type TLine
	Field x1:Float, y1:Float, x2:Float, y2:Float
	
	Function Create: TLine(x1:Float, y1:Float, x2:Float, y2:Float)
		Local tmp: TLine = New TLine
		tmp.x1 = x1
		tmp.y1 = y1
		tmp.x2 = x2
		tmp.y2 = y2
		Return tmp
	End Function
End Type

Type TBall
	Field pos:TPoint
	Field pos0:TPoint
	Field edgePos:tpoint
	Field size:Double
	Field tracking:Int 'Bool
	
	Field found:Int
	Field isComputed:Int
		
	Function Create:TBall(pos:tpoint, size:Double)
		Local tmp:TBall = New TBall
		tmp.pos = pos
		tmp.pos0 = pos
		tmp.edgePos = pos
		tmp.size = size
		tmp.tracking = False
		Return tmp
	End Function
End Type

Type TMetaballsystem
	Field balls:tball[]
	Field goo:Double
	Field threshold:Double
	Field minSize:Double
	Field oneOverGoo:Double
	Field minOverThresPowGoo:Double
	
	Field gridSiz:Float = 3.0
	Field gridW:Int = resX / gridSiz
	Field gridH:Int = resY / gridSiz
	
	'Field computed:Float Ptr
	Field computed:Float[,] 
	Field gridBytes:Int
	
	Field vUp:TPoint = TPoint.Create(0, -1)
	Field vDown:TPoint = TPoint.Create(0, 1)		
	Field vLeft:TPoint = TPoint.Create(-1, 0)
	Field vRight:TPoint = TPoint.Create(1, 0)
	
	Field variations_ccw:TPoint[15]
	Field borderList:TList
	
	Method New()
		gridBytes = 4*gridW*gridH '4 bytes for float
		'computed = Float Ptr( MemAlloc( gridBytes ) )
		computed = New Float[gridW,gridH]
		
		variations_ccw[%0000] = Null 'right
		variations_ccw[%0001] = vDown
		variations_ccw[%0010] = vLeft
		variations_ccw[%0011] = vLeft
		variations_ccw[%0100] = vRight
		variations_ccw[%0101] = VDown
		variations_ccw[%0110] = vLeft
		variations_ccw[%0111] = vLeft
		variations_ccw[%1000] = vUp
		variations_ccw[%1001] = vUp
		variations_ccw[%1010] = vUp
		variations_ccw[%1011] = vUp
		variations_ccw[%1100] = vRight
		variations_ccw[%1101] = vDown
		variations_ccw[%1110] = vRight
		' last is illegal
		
	End Method
	
	Function Create:TMetaballsystem(balls:TBall[], goo:Double, threshold:Double)
		Local tmp:TMetaballsystem = New TMetaballsystem
		
		tmp.balls = balls
		tmp.goo = goo
		tmp.threshold = threshold
		tmp.minSize = 1e32 'smallest ball
		tmp.oneOverGoo = 1.0/goo
		For Local b:TBall = EachIn balls
			If b.size < tmp.minSize Then tmp.minSize = b.size
		Next
		tmp.minOverThresPowGoo = (tmp.minSize/threshold)^(tmp.oneOverGoo)
		
		Return tmp
	End Function 
	
	Method setGridEmpty()
		'MemClear(computed, gridBytes )
		For Local i:Int = 0 Until gridW
		For Local j:Int = 0 Until gridH
			computed[i,j] = -1
		Next Next
	End Method
	
	Method marchingSquares()
		setGridEmpty()
		
		
		For Local ball:TBall = EachIn balls
			borderList = New TList
			wikiMarching(ball)
			
			Rem
			SetColor 255,0,0
			For Local i:Int = 0 Until (gridW-1)
			For Local j:Int = 0 Until (gridH-1)
				If i < 0 Or i > (gridW-1) DebugStop
				If j < 0 Or j > (gridH-1) DebugStop
				If computed[i,j] > 0 Plot i*gridSiz, j*gridSiz
				
			Next
			Next
			End Rem
			
			SetColor 0,255,0
			
			Local s# = gridSiz * .5
			Local old_pos:TPoint = Null
			Local startPos:TPoint = Null
			Local mvx#, mvy#
			If borderList<>Null
			For Local p:TPoint = EachIn borderList
				If True
					If old_pos = Null
						old_pos = p
						mvx = p.x
						mvy = p.y
						startPos = p
					Else
						Local lx# = lerp(old_pos.x, p.x, .5)
						Local ly# = lerp(old_pos.y, p.y, .5)
						
						DrawLine(mvx,mvy,lx,ly)
						mvx=lx;
						mvy=ly;
						'Plot lx,ly
						'DrawLine old_pos.x, old_pos.y, p.x, p.y
						old_pos = p
					EndIf
				Else
					DrawRect p.x-s, p.y-s,gridSiz, gridSiz
				EndIf
			Next
			
			If startPos <> Null
				Local lx# = lerp(startPos.x, old_pos.x, .5)
				Local ly# = lerp(startPos.y, old_pos.y, .5)
				DrawLine mvx,mvy,lx,ly
			EndIf
			
			EndIf
		Next
		
	End Method
		
	Method getFirstPoint:TPoint()
		Local firstPoint:TPoint = Null
		For Local y:Int = 2 Until gridH-2 Step 1
		For Local x:Int = 2 Until gridW-2 Step 1
			If ismeta(x,y)
				firstPoint = TPoint.Create( x,y )
				Return firstPoint
			EndIf
		Next
		Next
		Return Null
	End Method

	Method getNextEdgePoint:TPoint(x:Int, y:Int)
		Local index:Int = getMarchVector(x,y )
		If index=0
			DebugLog "marching squares error. variations_ccw[0]" 
			DebugStop()
		Else If index=15
			DebugLog "marching squares error. variations_ccw[15]" 
			DebugStop()
		EndIf
		Local p:TPoint = variations_ccw[index]
		Return TPoint.Create( x + p.x, y + p.y )
	End Method
	
	Method wikiMarching(ball:TBall) 'http://en.wikipedia.org/wiki/Marching_squares	
		Local pos:TPoint = getFirstPoint()
		If pos = Null Return
		pos.x :-1; pos.y :-1 'back up 1
		Local loop:TPoint = pos.clone()
		
		Local iter:Int = 0
		While iter < 1000
			iter:+1
			
			If(loop<>Null)
				borderList.addLast( TPoint.Create(gridSiz*pos.x,gridSiz*pos.y) ) 
			EndIf
			
			pos = getNextEdgePoint( pos.x, pos.y )
			
			Local hitLoop:Int = Abs(pos.x-loop.x) < 0.01 And Abs(pos.y-loop.y) < 0.01
			If(loop And hitLoop) Exit
			
		Wend	
	End Method
	
	Method getMarchVector:Int(x:Int, y:Int)
		If x < 0 Or x > (gridW-2) DebugStop
		If y < 0 Or y > (gridH-2) DebugStop
	
		Local index:Int = %0000
		If( ismeta(x+1,y+1) ) 
			index = index | %0001
		EndIf
		
		If( ismeta(x,y+1) )
			index = index | %0010
		EndIf
		
		If( ismeta(x+1,y) )
			index = index | %0100
		EndIf
		
		If( ismeta(x,y) )
			index = index | %1000
		EndIf
		
		Return index
	End Method
	
	Method ismeta:Int(x:Int, y:Int)
		If x < 0 Or x > (gridW-1) DebugStop
		If y < 0 Or y > (gridH-1) DebugStop
	
		If( computed[x,y] < 0.0 ) 'has not been calced
			computed[x,y] = calcForce( TPoint.Create(x*gridSiz, y*gridSiz) )
			If computed[x,y] > threshold Return True
		Else
			If computed[x,y] > threshold
				Return True
			Else
				Return False
			EndIf
		EndIf
		
		Return False
	End Method
	
	'[g(r) = r^4 - r^2 + 0.25]
	Method calcForce:Float(pos:tpoint) 'called by stepOnceTowardsBorder
		?debug
		Plot pos.x, pos.y
		?
		Local f:Float = 0.0
		
		'Local rdiv:Float = 1.0/Sqr(resX*resX + resY*resY)
		'Local rdiv:Float = 1e-4
		
		For Local ball:tball = EachIn balls
			Local dx:Float = ball.pos.x-pos.x
			Local dy:Float = ball.pos.y-pos.y
			Local rSquared:Float = (dx*dx+dy*dy)
			'rSquared :*rdiv
			rSquared :/ (1000*ball.size*ball.size)
	
			If rSquared > (0.707*0.707)
			Else
				Local g:Float = rSquared*rSquared* - rSquared + 0.25
				f = f + g
			EndIf
			
		Next
		
		Return Sqr(f) 
	End Method
	
	'TODO: optimize away sqrt. also. use ryans optim of only adding influence of metaballs whitin a given radius
	Method calcForceOld:Double(pos:tpoint) 'called by stepOnceTowardsBorder
		'return the metaball fields force at point "pos"
		Local forceAtPoint:Double = 0.0
		
		For Local ball:tball = EachIn balls
			'### Formula (1)
			Local tmp:TPoint = TPoint.Create(ball.pos.x-pos.x,ball.pos.y-pos.y)
			Local distSq:Double = ( tmp.x*tmp.x + tmp.y*tmp.y )^goo
			Local dist:Double = Sqr(distSq)
			
			'If( dist > 0.707 ) Continue Else forceAtPoint :+ dist^4 - dist^2 + 0.25
			If( dist > 1e-6 ) 'prevent div by zero
				forceAtPoint :+ (ball.size) / dist
			Else
				forceAtPoint :+ 10000.0'"big number
			EndIf
		Next
		Return forceAtPoint
	End Method
	

End Type

Function lerp:Float(x1#,x2#,t#)
	Return x1 + (x2-x1)*t
End Function


